

def normalize(vec):
	from math import sqrt
	len = sqrt( (vec[0] ** 2) + (vec[1] ** 2) + (vec[2] ** 2) )
	return [vec[0] / len, vec[1] / len, vec[2] / len ]

def dotprod(v1, v2):
	return (v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2])


def findDistance(p1, p2):
	from math import sqrt
	a = p2[0] - p1[0]
	b = p2[1] - p1[1]
	c = sqrt(pow(a,2) + pow(b,2))
	return c

def findAngle(p1, p2):
	from math import pow, sqrt
	a = pow(p2[1] - p2[0], 2)
	b = pow(p1[1] - p1[0], 2)
	c = sqrt(a + b)
	
	if c == 0:
		return 0.0
	else:
		theradians = atan2(a, b)
	
		return theradians
	
def matMult(matA, matB):
	matC = [[0,0,0],[0,0,0],[0,0,0]]
	for i in range(3):
		for j in range(3):
			a = matA[i][0]*matB[0][j]
			b = matA[i][1]*matB[1][j]
			c = matA[i][2]*matB[2][j]
			matC[i][j] = a+b+c
	return matC


def track(object, target, speed):
	from math import cos, sin
	# Target is a point[x,y]
	
	ownO = object.getOrientation()
	pos = object.getPosition()

	oriX = [ ownO[0][0], ownO[1][0], ownO[2][0] ]
	oriY = [ ownO[0][1], ownO[1][1], ownO[2][1] ]
	tvect = normalize([ target[0] - pos[0], target[1] - pos[1], target[2]-pos[2] ])
	oriY = [-oriY[1], oriY[0], oriY[2] ]  #get perpendicular 
	dot = dotprod(oriY, tvect)
	oriY = [oriY[1], -oriY[0], oriY[2] ]  #get perpendicular 
	cross = dotprod(oriY, tvect)
	ar = ( (cross-1)-.1 )*speed
	if dot > 0.004:
		ar = -ar
	elif dot < -.004:
		ar = ar
	elif cross < 0:
		ar = ar
	else:
		ar = 0

	rot_matrix = [[cos(ar), -sin(ar), 0],[sin(ar), cos(ar), 0],[0, 0, 1]]

	object.setOrientation( matMult(object.getOrientation(), rot_matrix))
	